package team314;

import java.util.Collection;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.PriorityQueue;

import battlecode.common.Clock;
import battlecode.common.Direction;
import battlecode.common.GameActionException;
import battlecode.common.GameConstants;
import battlecode.common.MapLocation;
import battlecode.common.Robot;
import battlecode.common.RobotController;
import battlecode.common.RobotInfo;
import battlecode.common.RobotType;
import battlecode.common.Team;
import battlecode.common.TerrainTile;

public class Navigator
{
	public static final TerrainTile.TerrainType LAND = TerrainTile.TerrainType.LAND;
	public static final TerrainTile.TerrainType OFFMAP = TerrainTile.TerrainType.OFF_MAP;
	public static final Direction[] DIRECTIONS = Direction.values();
	
	private static final int[] archonOrthScanRight = { -6,-5,-4,-3,-2,-1, 0, 1, 2, 3, 4, 5, 6};
	private static final int[] archonOrthScanForward = {0, 3, 4, 5, 5, 5, 6, 5, 5, 5, 4, 3, 0};
	private static final int[] archonDiagScanRight = { -3,-2, 0, 0, 1, 2, 3, 3, 4, 4, 5, 5, 5, 5, 6, 5, 5};
	private static final int[] archonDiagScanForward = {5, 5, 6, 5, 5, 5, 5, 4, 4, 3, 3, 2, 1, 0, 0,-2,-3};
	
	private static final int[] scoutOrthScanRight = { -5,-4,-3,-2,-1, 0, 1, 2, 3, 4, 5};
	private static final int[] scoutOrthScanForward = {0, 3, 4, 4, 4, 5, 4, 4, 4, 3, 0};
	private static final int[] scoutDiagScanRight = { -3,-2, 0, 0, 1, 2, 3, 3, 4, 4, 4, 4, 5, 4, 4};
	private static final int[] scoutDiagScanForward = {4, 4, 5, 4, 4, 4, 4, 3, 3, 2, 1, 0, 0,-2,-3};
	
	private static final int[] bomberOrthScanRight = { -4, -3, -2, -1, 0, 1, 2, 3, 4 };
	private static final int[] bomberOrthScanForward = {0,  2,  3,  3, 4, 3, 3, 2, 0 };
	private static final int[] bomberDiagScanRight = { -2, 0, 0, 1, 2, 2, 3, 3, 3, 4, 3 };
	private static final int[] bomberDiagScanForward = {3, 4, 3, 3, 3, 2, 2, 1, 0, 0, -2};
	
	private static final int[] soldierOrthScanRight = { -3,-2,-1, 0, 1, 2, 3};
	private static final int[] soldierOrthScanForward = {0, 2, 2, 3, 2, 2, 0};
	private static final int[] soldierDiagScanRight =  {-2, 0, 0, 1, 2, 2, 2, 3};
	private static final int[] soldierDiagScanForward =  {2, 3, 2, 2, 2, 1, 0, 0};
	
	private static final int[] sniperOrthScanRight = {-2,-1, 0, 1, 2};
	private static final int[] sniperOrthScanForward = {3, 3, 4, 3, 3};
	private static final int[] sniperDiagScanRight = {0, 0, 1, 2, 2, 3, 3, 3, 4};
	private static final int[] sniperDiagScanForward = {4, 3, 3, 3, 2, 2, 1, 0, 0};
		
	private static final int[] mortarOrthScanRight = sniperOrthScanRight;
	private static final int[] mortarOrthScanForward = {2, 2, 3, 2, 2};
	private static final int[] mortarDiagScanRight = {0, 0, 1, 2, 2, 2, 3};
	private static final int[] mortarDiagScanForward = {3, 2, 2, 2, 1, 0, 0};
	
	/**
	 * Cache during construction
	 */
	public int origX;
	public int origY;
	private int[] OR;
	private int[] OF;
	private int[] DR;
	private int[] DF;
	private final RobotController myRC;
	private final Team myTeam;
	private final RobotType myType;
		
	/**
	 * Update these after every move
	 */
	public MapLocation currLoc;
	public int currX;
	public int currY;
	private boolean moved = false;
	
	/**
	 * Update this after every direction change
	 */
	public Direction currDir;		
	private boolean directionChanged = false;
	
	public HashSet<MapLocation> map;
	
	public Navigator(RobotType rt, RobotController rc)
	{		
		// initial caches
		myRC = rc;
		currLoc = myRC.getLocation();
		currX = currLoc.getX();
		currY = currLoc.getY();
		origX = currX;
		origY = currY;
		currDir = myRC.getDirection();
		myTeam = myRC.getTeam();
		myType = rt;
		
		// map that will store all the obstacle terrains
		map = new HashSet<MapLocation>();
		// linkedlist that stores all the found neutral towers
		found_towers = new LinkedList<MapLocation>();
		
		bugNav = new BugNav();
		astarNav = new AstarNav();
		
		// set scanning data
		switch (rt)
		{
			case ARCHON:
				DR = archonDiagScanRight;
				DF = archonDiagScanForward;
				OR = archonOrthScanRight;
				OF = archonOrthScanForward;
				break;
			case SOLDIER:
				DR = soldierDiagScanRight;
				DF = soldierDiagScanForward;
				OR = soldierOrthScanRight;
				OF = soldierOrthScanForward;
				break;
			case SCOUT:
				DR = scoutDiagScanRight;
				DF = scoutDiagScanForward;
				OR = scoutOrthScanRight;
				OF = scoutOrthScanForward;
				break;
			case MORTAR:
				DR = mortarDiagScanRight;
				DF = mortarDiagScanForward;
				OR = mortarOrthScanRight;
				OF = mortarOrthScanForward;
				break;
			case SNIPER:
				DR = sniperDiagScanRight;
				DF = sniperDiagScanForward;
				OR = sniperOrthScanRight;
				OF = sniperOrthScanForward;
				break;
			case BOMBER:
				DR = bomberDiagScanRight;
				DF = bomberDiagScanForward;
				OR = bomberOrthScanRight;
				OF = bomberOrthScanForward;
		}
		// full scan for surrounding terrain
		try 
		{
			initialScan(rt);
		} 
		catch (GameActionException e)
		{			
			e.printStackTrace();
		}		
	}
	
	private void initialScan(RobotType type) throws GameActionException
	{   
		int radius = type.sensorRadius();
		for (int x = -radius; x <= radius; x++)
	    {
			for (int y = -radius; y <= radius; y++)
			{
				MapLocation temp = new MapLocation(currX + x, currY + y);
				TerrainTile tile = myRC.senseTerrainTile(temp);
				TerrainTile.TerrainType tType;
				if (tile != null 
						&& (tType = tile.getType()) != LAND 
						&& tType != OFFMAP) 
					map.add(temp);
			}
	    }    
		
		for (Robot r : myRC.senseNearbyGroundRobots())
	    {           
	    	RobotInfo ri = myRC.senseRobotInfo(r);
	    	if (ri.type == RobotType.TOWER)
	    	{	    	  
	    		map.add(ri.location);
	    		if (ri.team != myTeam)
	    		{
	    			
	    		}
	    	}      
	    }		
	}
	
	private void scanTerrain() throws GameActionException
	{
		if (currDir.isDiagonal())
			partialScan(DR, DF);
		else
			partialScan(OR, OF);
	}
	
	private void partialScan(int[] scanRight, int[] scanForward) throws GameActionException
	{		
	    MapLocation temp;
	    switch (currDir)
	    {
	      	case NORTH_EAST:
	      	case NORTH:
	      		for(int i = 0; i < scanRight.length; i++)
	      		{
	      			temp = new MapLocation(currX + scanRight[i], currY - scanForward[i]);
	      			TerrainTile tile = myRC.senseTerrainTile(temp);
	      			TerrainTile.TerrainType tType;
					if (tile != null 
							&& (tType = tile.getType()) != LAND 
							&& tType != OFFMAP) 
						map.add(temp);         
	      		}
	      		break;
	      	case SOUTH_EAST:
	      	case SOUTH:
	      		for(int i = 0; i < scanRight.length; i++)
		        {
		          temp = new MapLocation(currX + scanRight[i], currY + scanForward[i]);
		          TerrainTile tile = myRC.senseTerrainTile(temp);
		          TerrainTile.TerrainType tType;
		          if (tile != null 
							&& (tType = tile.getType()) != LAND 
							&& tType != OFFMAP) 
						map.add(temp);
		        }
	      		break;
	      	case EAST:
		        for(int i = 0; i < scanRight.length; i++)
		        {
		          temp = new MapLocation(currX + scanForward[i], currY + scanRight[i]);
		          TerrainTile tile = myRC.senseTerrainTile(temp);
		          TerrainTile.TerrainType tType;
		          if (tile != null 
							&& (tType = tile.getType()) != LAND 
							&& tType != OFFMAP) 
						map.add(temp);
		        }
		        break;
	      	case NORTH_WEST:
		        for(int i = 0; i < scanRight.length; i++)
		        {
		          temp = new MapLocation(currX - scanRight[i], currY - scanForward[i]);
		          TerrainTile tile = myRC.senseTerrainTile(temp);
		          TerrainTile.TerrainType tType;
		          if (tile != null 
							&& (tType = tile.getType()) != LAND 
							&& tType != OFFMAP) 
						map.add(temp);
		        }
		        break;
	      	case WEST:
		        for(int i = 0; i < scanRight.length; i++)
		        {
		          temp = new MapLocation(currX - scanForward[i], currY + scanRight[i]);
		          TerrainTile tile = myRC.senseTerrainTile(temp);
		          TerrainTile.TerrainType tType;
		          if (tile != null 
							&& (tType = tile.getType()) != LAND 
							&& tType != OFFMAP) 
						map.add(temp);
		        }
		        break;
	      	case SOUTH_WEST:
		        for(int i = 0; i < scanRight.length; i++)
		        {
		          temp = new MapLocation(currX - scanRight[i], currY + scanForward[i]);
		          TerrainTile tile = myRC.senseTerrainTile(temp);
		          TerrainTile.TerrainType tType;
		          if (tile != null 
							&& (tType = tile.getType()) != LAND 
							&& tType != OFFMAP) 
						map.add(temp);
		        }
		        break;
	    }
	    
	    for (Robot r : myRC.senseNearbyGroundRobots())
	    {           
	    	RobotInfo ri = myRC.senseRobotInfo(r);
	    	if (ri.type == RobotType.TOWER)
	    	{	    	  
	    		map.add(ri.location);
	    		if (ri.team != myTeam)
	    			found_towers.add(ri.location);
	    	}      
	    }
	    
	}
	
	private int maploc_compareTo(MapLocation a, MapLocation b)
	{
		if (a.getY() < b.getY())
		{
			return -1;
		}
		else if (a.getY() > b.getY())
		{
			return 1;
		}
		else
		{
			if (a.getX() < b.getX())
				return -1;
			else if (a.getX() > b.getX())
				return 1;
			return 0;
		}
	}
	
	private void sort_maploc_array(MapLocation[] m)
	{
		int i, j;
		int min;
		MapLocation temp;

		for (i = 0; i < m.length - 1; i++)
		{
			min = i;
			for (j = i+1; j < m.length; j++)
			{
				if (maploc_compareTo(m[j], m[min]) < 0)
					min = j;
			}
			temp = m[i];
			m[i] = m[min];
			m[min] = temp;
		}
	}
	
	public void debug_map()
	{
		MapLocation[] obstacles = new MapLocation[map.size()];
		int i = 0;
		for (MapLocation x : map)
		{
			obstacles[i++] = x;
		}
		sort_maploc_array(obstacles);
		System.out.println("Current Location: " + (currY - origY) + ", " + (currX - origX));
		for(MapLocation x : obstacles)
		{
			int dx = x.getX() - currX;
			int dy = x.getY() - currY;
			System.out.println(dy + ", " + dx);
		}
		System.out.println("Total Size: " + map.size());
	}
			
	public enum MovementInstruction
	{
		BRAINLESS, TOWER, LOCATION, FOLLOW, ENEMY, BASE, RETREAT, SWARM, IDLE
	}
	
	public enum MovementStatus
	{
		CALCULATING, IN_PROGRESS, BLOCKED, SUCCESS, FAILURE	
	}
	
	public MovementInstruction mvmtInstr = MovementInstruction.IDLE;
	public MovementStatus mvmtStatus = MovementStatus.IN_PROGRESS;
	
	public void navigate() throws GameActionException
	{
		if (mvmtStatus == MovementStatus.SUCCESS || mvmtStatus == MovementStatus.FAILURE)
		{
			// the last instruction is completed or failed
			mvmtInstr = MovementInstruction.IDLE;
			mvmtStatus = MovementStatus.IN_PROGRESS;
		}
		
		switch (mvmtInstr)
		{
			case BRAINLESS:
				move_brainless();
				break;
			case TOWER:
				move_to_tower();
				break;
			case LOCATION:
				move_location();
				break;
			case FOLLOW:
				break;
			case ENEMY:
				break;
			case BASE:
				move_to_base();
				break;
			case RETREAT:
				break;
			case IDLE:
				return;
		}
	}
	
	public void update() throws GameActionException
	{
		if (moved)
		{
			currLoc = myRC.getLocation();
			currX = currLoc.getX();
			currY = currLoc.getY();
			scanTerrain();
			moved = false;
		}
		if (directionChanged)
		{
			currDir = myRC.getDirection();
			directionChanged = false;
		}
	}
	
	/*
	 * Brainless
	 */
	
	private int brainless_counter;
	
	public void becomeBrainless(int counter)
	{
		mvmtInstr = MovementInstruction.BRAINLESS;
		brainless_counter = counter;
	}
	
	private void move_brainless() throws GameActionException
	{
		if (myRC.isMovementActive())
			return;
		
		if (brainless_counter > 1)
		{
			brainless_counter--;			
		}
		else if (brainless_counter == 0)
		{			
			mvmtStatus = MovementStatus.SUCCESS;
			return;
		}
		
		// can i move in current direction?
		if (myRC.canMove(currDir))
		{
			myRC.moveForward();
			moved = true;
		}
		else
		{
			myRC.setDirection(currDir.rotateRight());
			directionChanged = true;
		}
		
	}		
	
	private MapLocation destination;	
	// 0 - none
	// 1 - bugNav
	// 2 - Astar
	// 3 - Both
	private int path_history;
	// 0 - none
	// 1 - bugNav
	// 2 - astar
	private int path_method;
	// astar stuff
	private AstarNav astarNav;
	// bugnav stuff
	private BugNav bugNav;
	
	private Direction directionTo(MapLocation from, MapLocation to)
	{
	    int toX = to.getX();  
	    int toY = to.getY();
	    int fromX = from.getX();
	    int fromY = from.getY();
	    
	    if(fromX < toX)
	    {
	    	if(fromY < toY) return Direction.SOUTH_EAST;
	    	if(fromY > toY) return Direction.NORTH_EAST;
	    	else return Direction.EAST;
	    }
	    if(fromX > toX)
	    {
	    	if(fromY < toY) return Direction.SOUTH_WEST;
	    	if(fromY > toY) return Direction.NORTH_WEST;
	      	else return Direction.WEST;
	    }
	    if(fromY < toY) return Direction.SOUTH;
	    else return Direction.NORTH;
	}  
	
	public void moveToLocation(MapLocation loc) throws NullPointerException, GameActionException
	{
		mvmtInstr = MovementInstruction.LOCATION;
		// initial tests
		// have we reached our destination?
		if (loc.equals(currLoc))
		{
			mvmtStatus = MovementStatus.SUCCESS;
			return;
		}
		
		// is our location unreachable?
		if (map.contains(loc))
		{
			// at least it's in sight
			mvmtStatus = MovementStatus.SUCCESS;
			return;
		}
		
		destination = loc;
		path_method = 0;		
		path_history = 0;
	}		
	
	private void debug_bytecode()
	{
		System.out.println("Moving: " + myRC.isMovementActive() +  "Rounds: " + myRC.getRoundsUntilMovementIdle() + " Bytecode Remaining: " 
			+ String.valueOf(GameConstants.BYTECODES_PER_ROUND - Clock.getBytecodeNum()));
	}
	
	class BugNode
	{
	    Direction d;
	    MapLocation mloc;
	    BugNode parent;
	    BugNode child;
	      
	    public BugNode(Direction _d, MapLocation _mloc, BugNode p)
	    {
	     	d = _d;
	     	mloc = _mloc;
	     	parent = p;
	    }
	}
	  	
	class BugNav 
	{               		
		public int maxTraceDist = 16;
		public int maxWaitPeriod = 3;
		
		boolean failed;
		
		// 0 - none
		// 1 - left
		// 2 - right
	    int traceStatus;
	    
	    // 0 - none
	    // 1 - left
	    // 2 - right
	    // 3 - both
	    int traceHistory;
	    
	    // 0 - normal
	    // 1 - tracing
	    // 2 - returning
	    int state; 	    	    
	    
	    int currTraceDist;
	    int waitingPeriod;
	    
	    BugNode fork; 
	    MapLocation forkLoc;
	    	  	    
	    public void reset()
	    {
	    	failed = false;
	    	traceStatus = 0;
	    	traceHistory = 0;
	    	state = 0;
	    	currTraceDist = 0;
	    	waitingPeriod = 0;
	    	fork = null;
	    }
	    
	    // only called while tracing
	    private void back_to_fork()
	    {
	    	switch (traceStatus)
			{
				case 1:
					traceHistory++;
					break;
				case 2:
					traceHistory += 2;
					break;
				case 0:
					// shouldn't Happen
			}
			traceStatus = 0;
			state = 2;
	    }	    
	    
	    private void bugMove(Direction targetDir) throws GameActionException 
		{	    	
			if (myRC.isMovementActive())
			{
				// There's no point in continuing
				return;
			}
			
			// tracing
			if (state == 1) 
			{      
				// if we tried both ways
				if (traceHistory == 3)
				{
					failed = true;
					return;
				}
				
				// if we went too far without normalizing
				if (currTraceDist > maxTraceDist) 
				{
					back_to_fork();
					return;
				}        
					
				Direction d = currDir;
				d = (traceStatus == 1) ? d.rotateRight() : d.rotateLeft();
				int i = 0;
				while (myRC.canMove(d)) 
				{					
					i++;
					
					if (d == targetDir)
					{
						// back to normal
						state = 0;
						fork = null;
						return;
					}
					
					d = (traceStatus == 1) ? d.rotateRight() : d.rotateLeft();									
				}   
				
				i = 0;
				while (!myRC.canMove(d)) 
				{
					if (i++ == 8)
					{		
						mvmtStatus = MovementStatus.BLOCKED;
						return;
					}      
					
					d = (traceStatus == 1) ? d.rotateLeft() : d.rotateRight();
				}
																
				// if we are retracing steps
				if (forkLoc != null && forkLoc.equals(currLoc.add(d))) 
				{
					back_to_fork();
					return;
				}									
				
				if (fork == null) 
				{
					fork = new BugNode(d, currLoc, null);
					forkLoc = fork.mloc;
				}
				else 
					fork = fork.child = new BugNode(d, currLoc, fork);    
								
				if (currDir == d)
				{
					myRC.moveForward();
					moved = true;
					bugNav.currTraceDist++;
				}
				else
				{
					myRC.setDirection(d);
					directionChanged = true;
				}								
			} 
			else if (state == 2) 
			{
				if (fork == null) 
				{
					set_tracing_status(targetDir);
				}
				else 
				{
					Direction d = fork.d.opposite();
					if (myRC.canMove(d))
					{
						if (currDir == d)
						{
							myRC.moveForward();
							moved = true;
						}
						else
						{
							myRC.setDirection(d);
							directionChanged = true;
						}
					}
					fork = fork.parent;
				}
			} 			
			else
			{      
				if (myRC.canMove(targetDir))
				{
					if (currDir == targetDir)
					{
						myRC.moveForward();
						moved = true;
					}
					else
					{
						myRC.setDirection(targetDir);
						directionChanged = true;
					}
				}
				else // If we can't move there
				{
					// If it's an obstacle
					if (map.contains(currLoc.add(targetDir)))
					{
						set_tracing_status(targetDir);
					}
					// it must be someone there blocking us
					else if (waitingPeriod++ >= maxWaitPeriod)
					{
						set_tracing_status(targetDir);
						waitingPeriod = 0;
					}    
				}
			}		
		}
	    
	    private void set_tracing_status(Direction d)
		{
			bugNav.state = 1;
			bugNav.currTraceDist = 0;
			switch (bugNav.traceHistory)
			{
				case 0:
					switch (d)
					{
						case NORTH:
						case WEST:							
						case NORTH_WEST:
						case SOUTH_EAST:	
							bugNav.traceStatus = 1;
							break;
						case NORTH_EAST:
						case SOUTH:
						case SOUTH_WEST:
						case EAST:
							bugNav.traceStatus = 2;								
					}					
					break;
				case 1:
					bugNav.traceStatus = 2;
					break;
				case 2:
					bugNav.traceStatus = 1;
			}
		}
	    
	}  
	
	class MapNode implements Comparable<MapNode>
	{
		public MapLocation loc;
		public MapNode parent;
		public MapNode child;
		public int F;
		public int G;
		public int H;

		public MapNode()
		{
		}

		public MapNode(MapLocation ml, MapNode p, int g, int h)
		{
			loc = ml;
			parent = p;
			G = g;
			H = h;
			F = G+H;
		}

		public void setG(int g)
		{
			F = (G = g) + H;
		}

		public void setH(int h)
		{
			F = G + (H = h);
		}
	 
		@Override
		public int compareTo(MapNode o)
		{
			if (this.F < o.F) return -1;
			if (this.F == o.F) return 0;
			return 1;
		}
	}
		
	class AstarNav
	{
		private MapLocation end;
		private int wait_counter;
		private MapNode path;	
		private int astar_cutoff;	
		private MapLocation[] previous_calc_locations;
		private boolean failed;
		
		public void debug_astar_cutoff()
		{
			System.out.println("Astar Cutoff: " + astar_cutoff);
		}
		
		private boolean going_in_circles()
		{
			int d1 = currLoc.distanceSquaredTo(previous_calc_locations[1]);
			int d2 = currLoc.distanceSquaredTo(previous_calc_locations[0]);
			
			d1 = (int) Math.sqrt(d1);
			d2 = (int) Math.sqrt(d2);
			
			if (d1 + d2 > 10)
				return false;
			else
				return true;
		}
		
		// This calculates the path to the destination field
		private void calculate_path(boolean wait) throws NullPointerException, GameActionException
		{		
			
			// if we're not getting anywhere
			if (previous_calc_locations[0] != null && previous_calc_locations[1] != null)
			{
				if (going_in_circles())
				{	
					// try harder
					astar_cutoff += 20;				
				}			
				else
				{					
					if (astar_cutoff > 10)
						astar_cutoff -= 10;								
				}
			}
			
			debug_astar_cutoff();
			
			// caching previous locations
			previous_calc_locations[1] = previous_calc_locations[0];
			previous_calc_locations[0] = currLoc;
			
			mvmtStatus = MovementStatus.CALCULATING;
						
			path = astar_pathfind(end, wait, astar_cutoff);
			if (path != null)
			{
				while (path.parent != null)
			    {
					path.parent.child = path;
					path = path.parent;
			    }
				if (path.child != null)
					path = path.child;
			}
			else 
			{
				failed = true;
			}
		}
				
		public void reset(MapLocation dest) throws NullPointerException, GameActionException
		{
			failed = false;
			end = dest;
			astar_cutoff = 7;
			previous_calc_locations = new MapLocation[2];
			calculate_path(true);	
		}
		
		public void astarMove() throws NullPointerException, GameActionException
		{
			// follow along our pre-calculated path, until we encounter some problems
			if (path == null)					
			{			
				calculate_path(true);	
				
				if (failed)
					return;
			}						
						
			Direction d = directionTo(currLoc, path.loc);
									
			if (myRC.canMove(d))
			{					
				if (myRC.isMovementActive())								
					return;
				
				if (currDir == d)
				{																								
					myRC.moveForward();
					moved = true;
					path = path.child;
				}
				else
				{																				
					myRC.setDirection(d);
					directionChanged = true;
				}					
			}
			else
			{	
				// is someone blocking us?
				if (myRC.senseGroundRobotAtLocation(currLoc.add(d)) != null)
				{
					// have we been waiting already?
					if (wait_counter > 7)
					{
						wait_counter = 0;						
						calculate_path(false);					
						return;
					}
					else
					{
						// Let's wait for 1 turn
						wait_counter++;
						mvmtStatus = MovementStatus.BLOCKED;
						return;
					}
				}												
				calculate_path(true);							
			}			
		}		
		
		private int abs(int x) { return x < 0 ? -x:x; }
		
		public void debug_mapnode(MapNode n, MapLocation last)
		{
			if (n == null) return;
			System.out.println(directionTo(last, n.loc));
			debug_mapnode(n.child, n.loc);
		}

		private MapNode astar_pathfind(MapLocation end, boolean wait, int lookAheadLimit) throws GameActionException,
	          NullPointerException
	    {
			// if our destination is ourself or unreachable, return null for now
			if (end.equals(currLoc) || map.contains(end))
				return null;
			// List of candidates to be considered
			HashMap<MapLocation, MapNode> open = new HashMap<MapLocation, MapNode>();
			// List of locations eliminated
			HashMap<MapLocation, MapNode> closed = new HashMap<MapLocation, MapNode>();
			// Same as the open list, but prioritized
			PriorityQueue<MapNode> openHeap = new PriorityQueue<MapNode>(160);
			
		    int endX = end.getX();
		    int endY = end.getY();
		    // Loop counter
		    int currIt = 0;
		    
		    MapNode currNode = null;
		    MapNode startNode = new MapNode(currLoc, null, 0,10*(abs(endX-currX)+abs(endY-currY)));
		    
		    open.put(currLoc, startNode);
		    openHeap.offer(startNode);
		    
		    if (!wait)
		    {
			    for (Direction d : DIRECTIONS)
			    {
			    	if (d == Direction.NONE || d == Direction.OMNI) break;
			    	MapLocation adj = currLoc.add(d);
			    	if (myRC.canSenseSquare(adj) && myRC.senseGroundRobotAtLocation(adj) != null) 
			    	{
			    		if (end.equals(adj))
			    			return null;
			    		closed.put(adj, new MapNode());
			    	}     
			    }
		    }
		    
		    while (true)
		    {
		    	// check if we've reached the end
		    	MapNode tempEnd = closed.get(end);
		    	// if we have, return it
		    	if (tempEnd != null) 
		    		return tempEnd;   
		    	
		    	// if we're out of candidates, then we failed
		    	if (open.isEmpty()) 
		    		return null;
		    	
		    	// get the best candidate
		    	currNode = openHeap.poll();
		    	open.remove(currNode.loc);
		    	closed.put(currNode.loc, currNode);
		    	
		    	// if we reached look ahead limit, just go with this one
		    	if (++currIt > lookAheadLimit) 
		    		return currNode;
		    	
		    	for (Direction d : DIRECTIONS)
		    	{
			        if (d == Direction.NONE || d == Direction.OMNI) 
			        	break;
			        
			        MapLocation temp = currNode.loc.add(d);
			        
			        if (closed.get(temp) != null) 
			        	continue;
			        
			        int gVal = (d.isDiagonal())? 14:10;
			        MapNode m = open.get(temp);
			        
			        if (m != null)
			        {
			        	int newG = currNode.G + gVal;
			        	if (newG < m.G)
			        	{
			        		m.parent = currNode;
				            m.setG(newG);
				            openHeap.remove(m);
				            openHeap.offer(m);			            
			        	}
			        	continue;
			        }
			        
			        if (map.contains(temp))
			        {
			        	closed.put(temp, new MapNode());
			        	continue;
			        }
			        else
			        {
			        	MapNode candidate = new MapNode(temp, currNode, gVal+currNode.G,10*(abs(endX-temp.getX())+abs(endY-temp.getY())));
			        	open.put(temp, candidate);
			        	openHeap.offer(candidate);          
			        }
			        
		    	}
		    }
	    }
	}
					
	
	
	private void move_location() throws GameActionException
	{
		// have we reached our destination?
		if (destination.equals(currLoc))
		{
			mvmtStatus = MovementStatus.SUCCESS;
			path_method = 0; //reset
			return;
		}
		
		// is our location unreachable?
		if (map.contains(destination))
		{
			// for now
			// at least it's in sight
			mvmtStatus = MovementStatus.SUCCESS;
			path_method = 0; //reset
			return;
		}
		
		if (path_method == 0)
		{						
			// initialization
			initPathing(2);
		}
		
		switch (path_method)
		{			
			case 1:				
				bugNav.bugMove(directionTo(currLoc, destination));	
				
				if (bugNav.failed)
				{		
					path_history ^= 0x1;								
					switch (path_history)
					{						
						case 1:
							initPathing(1);
							break;
						case 3:
							mvmtStatus = MovementStatus.FAILURE;
							return;
					}																				
				}				
				break;
			case 2:
				astarNav.astarMove();
				if (astarNav.failed)
				{
					path_history ^= 0x2;
					switch (path_history)
					{						
						case 2:
							initPathing(1);
							break;
						case 3:
							mvmtStatus = MovementStatus.FAILURE;
							return;
					}		
				}
		}
		
	}
	
	private void initPathing(int method) throws NullPointerException, GameActionException
	{
		path_method = method;
		switch (path_method)
		{
			case 1:
				bugNav.reset();					
				break;
			case 2:			
				astarNav.reset(destination);
		}
	}
		
	private LinkedList<MapLocation> found_towers;
	private MapLocation curr_tower_target;
		
	public void moveToTower()
	{
		mvmtInstr = MovementInstruction.TOWER;
		bugNav.reset();
	}
	
	private void move_to_tower() throws GameActionException
	{
		// this function is defined as success when we are in sight of a neutral tower
		// In the beginning, we don't have the specific locations of towers, only the
		// direction to the nearest neutral tower. 
		
		// To check whether we succeeded or not, we must maintain a linkedlist of found towers
		// We then pick one of those towers (the first one) and move towards it until it is
		// in our sight. How do we know if it is in our sight? if the distance to it is
		// less than or equal to our sensor radius.
		boolean have_a_target = false;
		
		if (curr_tower_target == null)
		{
			// means we haven't picked a tower yet
			
			if (found_towers.size() > 0)
			{
				// surprise! we have found some!
				curr_tower_target = pick_closest(currLoc, found_towers);
				have_a_target = true;
				// check if we are within sight
				if (currLoc.distanceSquaredTo(curr_tower_target) <= (myType.sensorRadius() * myType.sensorRadius()))
				{
					mvmtStatus = MovementStatus.SUCCESS;
					return;
				}
				astarNav.reset(curr_tower_target);
			}
			
		}
		else
		{
			have_a_target = true;
			// check if we are within sight
			if (currLoc.distanceSquaredTo(curr_tower_target) < (myType.sensorRadius() * myType.sensorRadius()))
			{
				mvmtStatus = MovementStatus.SUCCESS;
				return;
			}
		}
		
		//TODO: make this fail safe by using Astar + BugNav for both stages
		// for now...
		if (have_a_target)
		{
			astarNav.astarMove();
			if (astarNav.failed)
			{
				mvmtStatus = MovementStatus.FAILURE;
				return;
			}
		}
		else
		{
			bugNav.bugMove(myRC.senseClosestUnknownTower());
			if (bugNav.failed)
			{
				mvmtStatus = MovementStatus.FAILURE;
				return;
			}
		}
	}
	
	private MapLocation pick_closest(MapLocation start, MapLocation[] candidates)
	{
		if (candidates.length == 0)
			return null;
		
		MapLocation closest = null;
		int closestD = -1;;
		
		for (MapLocation loc : candidates)
		{
			if (closest == null)
			{
				closest = loc;
				closestD = loc.distanceSquaredTo(start);
				continue;
			}
			
			if (loc.distanceSquaredTo(start) < closestD)
			{
				closest = loc;
				closestD = loc.distanceSquaredTo(start);
			}
		}
		
		return closest;
	}
	
	private MapLocation pick_closest(MapLocation start, Collection<MapLocation> candidates)
	{
		if (candidates.size() < 1)
			return null;
		
		MapLocation closest = null;
		int closestD = -1;;
		
		for (MapLocation loc : candidates)
		{
			if (closest == null)
			{
				closest = loc;
				closestD = loc.distanceSquaredTo(start);
				continue;
			}
			
			if (loc.distanceSquaredTo(start) < closestD)
			{
				closest = loc;
				closestD = loc.distanceSquaredTo(start);
			}
		}
		
		return closest;
	}
	
	private MapLocation curr_base_tower;
	private MapLocation curr_base_target;
	private boolean reached_target;
	
	public void moveToBase() throws NullPointerException, GameActionException
	{					
		// our goal is to move to a spot where we can spawn
		// first we figure out the closest base		
		
		if (curr_base_tower == null)
		{
			curr_base_tower = pick_closest(currLoc, myRC.senseAlliedTowers());
			
			if (curr_base_tower == null)
			{
				// basically, we don't have a tower left
				// SUICIDE!!!
				myRC.suicide();
			}
			
			Direction d = currDir;
			MapLocation temp = curr_base_tower.add(d);
			
			while (map.contains(temp))
			{
				d.rotateLeft();
				temp = curr_base_tower.add(d);
			}
			
			curr_base_target = temp;
			
			// basically, now we use move_location code, but override it with
			// the base movement instruction -- what a hack
			moveToLocation(curr_base_target);			
		}
		reached_target = false;
		mvmtInstr = MovementInstruction.BASE;
	}
	
	private void move_to_base() throws GameActionException
	{
		// check if our current target still belongs to us
		boolean ours = false;
		for (MapLocation m : myRC.senseAlliedTowers())
		{
			if (m.equals(curr_base_tower))
			{
				ours = true;
				break;
			}
		}
		
		if (!ours)
		{
			curr_base_tower = null;
			moveToBase();
			return;
		}
		
		// check to see if we are in range
		// we succeed if we are in spawn radius AND not too crowded
		if (myRC.canSpawn())
		{
			int occ = 0;
			boolean crowded = false;
			for (Direction d : DIRECTIONS)
		    {
		    	if (d == Direction.NONE || d == Direction.OMNI) break;
		    	MapLocation adj = currLoc.add(d);
		    	if (myRC.canSenseSquare(adj) && myRC.senseGroundRobotAtLocation(adj) != null)
		    	{		       	
		    		occ++;
		    		if (occ >= 1)
		    		{
		    			crowded = true;
		    			break;
		    		}
		    	}
		    }
			
			if (crowded)
			{				
				if (reached_target)
				{
					curr_base_target = pick_loc_inside_spawn_radius(curr_base_tower);
					moveToLocation(curr_base_target);				
					mvmtInstr = MovementInstruction.BASE;
				}
			}
			else
			{
				mvmtStatus = MovementStatus.SUCCESS;
				return;			
			}
		}
		
		move_location();
		
		if (mvmtStatus == MovementStatus.SUCCESS || mvmtStatus == MovementStatus.FAILURE)
		{
			// this means we are at the target loc
			reached_target = true;
			mvmtStatus = MovementStatus.IN_PROGRESS;
		}		
	}
	
	private MapLocation pick_loc_inside_spawn_radius(MapLocation tower) throws GameActionException
	{
		int radius = 6;
		
		if (myRC.canSenseSquare(tower))
		{
			radius = (int)myRC.senseTowerSpawnRadius(myRC.senseGroundRobotAtLocation(tower));			
		}
		
		int rand = Clock.getBytecodeNum();
		int x = rand % radius;
		int y = rand % (radius - x);		
			
		x = Clock.getBytecodeNum() % 2 == 0 ? x : -x;
		y = Clock.getBytecodeNum() % 2 == 0 ? y : -y;
		
		return new MapLocation(tower.getX() + x, tower.getY() + y);
	}
	
	
}
